//////////////////////////////////////////////////////////////////////////////
//
//  Created by Philip Mulcahy on 31/01/2012.
//  Copyright (c) 2012 Philip Mulcahy. All rights reserved.
//
//  This file is part of the note-recog library.
//
//  note-recog is free software: you can redistribute it and/or modify
//  it under the terms of version 3 of the GNU Lesser General Public License 
//  as published by the Free Software Foundation.
//
//  note-recog is distributed is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU General Public License for more details.
//
//  You should have received a copy of the GNU General Public License
//  along with note-recog.  If not, see <http://www.gnu.org/licenses/>.
//
//////////////////////////////////////////////////////////////////////////////

#include "Harmonic.h"
#include <cmath>
#include <cassert>

using namespace std;

size_t nextLower(float start);
size_t nextHigher(float start);

Harmonic::Harmonic( 
                   Slice::fft_iterator begin,
                   Slice::fft_iterator end,
                   float binWidthHz,
                   size_t iBin
                   ) :
  _begin(begin),
  _end(end),
  _bin(iBin),
  _binWidthHz(binWidthHz)
{
    const Slice::fft_iterator midIt = begin + iBin;
    assert( midIt > begin );
    assert( midIt < _end - 1 );
    
    const float left  = *(midIt - 1);
    const float mid   = *midIt;
    const float right = *(midIt + 1);
    
    const float swing = right - left;
    const float bend = 2.f * ( left + right - 2.f * mid );
    
    assert( ( 2.f * mid ) > ( left + right) );
    
    const float binOffset = swing / bend;
    
    _binPrecise = _bin - binOffset;
    
    _peak = mid - ( swing * swing ) / ( 2 * bend );
    
    const float root2on2 = 1/sqrt(2.f);
    const float targetLevel = _peak * root2on2;
    const float leftHalfPowerBin = findBin(true,_binPrecise,_peak,targetLevel);
    const float rightHalfPowerBin = findBin(false,_binPrecise,_peak,targetLevel);
    _halfPowerBw = rightHalfPowerBin - leftHalfPowerBin;
    _power = _halfPowerBw * _peak;
}

size_t nextLower(float start)
{
    const size_t st = static_cast<size_t>(start);
    if( st < start )
    {
        return start;
    }
    else
    {
        return start - 1;
    }
}

size_t nextHigher(float start)
{
    const size_t st = -static_cast<size_t>(-start);
    if( st > start )
    {
        return start;
    }
    else
    {
        return start + 1;
    }
}

float Harmonic::findBin(
                        const bool left,
                        const float startBin,
                        const float startHeight,
                        const float targetHeight
) const
{
    if( left && startBin == 0.f )
    {
        return 0.f;
    }
    
    const size_t binCount = _end - _begin;
    const size_t topBin = binCount - 1;
    if( !left && startBin == topBin )
    {
        return topBin;
    }
    
    assert( startBin > 0.f );
    
    const size_t farBin = left ? nextLower(startBin) : nextHigher(startBin);
    const Slice::fft_iterator far = _begin + farBin;
    assert( far < _end );
    const float farHeight = *far;
    if( farHeight <= targetHeight )
    {
        const float result =
            startBin 
            + ( ( targetHeight - startHeight ) / ( farHeight - startHeight ) ) 
            * ( farBin - startBin );
        
        return result;
    }

    return findBin(left, farBin, farHeight, targetHeight);
}

float Harmonic::freq() const 
{ 
    return _binPrecise * _binWidthHz;
}

float Harmonic::peak() const { return _peak; }

float Harmonic::halfPowerBw() const { return _halfPowerBw; }

float Harmonic::power() const { return _power; }

size_t Harmonic::bin() const { return _bin; }

float Harmonic::binPrecise() const { return _binPrecise; }

ostream& operator<<(ostream& os, const Harmonic& h)
{
    os 
    << "Harmonic{"
    << "n=" << h.bin()
    << ",np=" << h.binPrecise()
    << ",f=" << h.freq()
    << ",pk=" << h.peak()
    << ",pow=" << h.power()
    << ",bw=" << h.halfPowerBw()
    << "}";
    
    return os;
}


